/*
 * Version 2.0
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.templates.LogitechControllerButton;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 *
 * @author Jarrod Risley
 * 
 * Courtesy of Matt Khan's mind from the Aztechs.
 */
public class MagicalStateMachine extends CommandBase {
    
    /*
     * Global Class Variables
     */
    private StateMachineState currentState;
    
    private boolean isFinished;
    private boolean isButtonPressed;
    
    private int timesMovRan = 0;
    private int numShot;
    
    private static final double CLEAR_SHOOTER_TIME = 2;
    private static final double MOV_TIME = 0.6;
    private static final double SPIN_UP_TIME = 2;
    
    public static class StateMachineState {
        
        /*
         * Inner Class Variables
         */
        private int value;
        private static final int IDL = 0;
        private static final int ING = 1;
        private static final int REV = 2;
        private static final int MOV = 3;
        private static final int CLE = 4;
        private static final int SPI = 5;
        private static final int SHO = 6;
        
        public static final StateMachineState ILDE =
                new StateMachineState(IDL);
        
        public static final StateMachineState INGEST =
                new StateMachineState(ING);
        
        public static final StateMachineState REVERSE =
                new StateMachineState(REV);
        
        public static final StateMachineState MOVE_UP_ONE =
                new StateMachineState(MOV);
        
        public static final StateMachineState CLEAR_FLYWHEEL =
                new StateMachineState(CLE);
        
        public static final StateMachineState SPIN_UP_FLYWHEEL =
                new StateMachineState(SPI);
        
        public static final StateMachineState SHOOT =
                new StateMachineState(SHO);
        
        
        // INNER CLASS CONSTRUCTOR //
        
        
        private StateMachineState(int value) {
            this.value = value;
        } // end StateMachineState Constructor
         
        
        // INNER CLASS METHOD //
        
        
        /**
         * Gets the value of the state.
         * @return the value of the state.
         */
        public int getValue() {
            return value;
        } // end getValue
        
    } // end StateMachineState
    
    public MagicalStateMachine() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        
        requires(shooter);
        requires(intake);
        
    } // end MagicalStateMachine Constructor

    // Called just before this Command runs the first time
    protected void initialize() {
        
        isFinished = false;
        currentState = StateMachineState.INGEST;
        
    } // end initialize

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        
        isButtonPressed = oi.getCoDriverController().getButtonA();
        
        switch (currentState.getValue()) {
            
            /*
             * Idle State
             */
            
            case StateMachineState.IDL:
                intake.intake(RobotMap.INTAKE_FRIZBEE);
                shooter.stop();
                break;
                
            /*
             * Ingest State
             */
                
            case StateMachineState.ING:
                
                if (isButtonPressed == true) {
                    
                    do {

                        shooter.manualTopIndexerDrive(0);
                        intake.intake(RobotMap.INTAKE_FRIZBEE);
                        shooter.ingest();

                    } while (!shooter.checkSwitch(2));

                    currentState = StateMachineState.MOVE_UP_ONE;
                    break;
                    
                } else
                    break;
            
            /*
             * Move Up One State
             */
                
            case StateMachineState.MOV:
                
                if (isButtonPressed == true) {
                    
                    double startTime = Timer.getFPGATimestamp();

                    shooter.stop();
                    intake.intake(RobotMap.STOP_INTAKE);

                    if (timesMovRan < 2) {

                        do {
                            shooter.manualTopIndexerDrive(1);
                        } while (startTime + MOV_TIME > Timer.getFPGATimestamp());

                        currentState = StateMachineState.INGEST;
                        timesMovRan++;
                        break;

                    } else if (timesMovRan == 3) {

                        do {
                            shooter.manualTopIndexerDrive(0.3);
                        } while (startTime + MOV_TIME > Timer.getFPGATimestamp());

                        currentState = StateMachineState.INGEST;
                        timesMovRan++;
                        break;

                    } else {

                        currentState = StateMachineState.CLEAR_FLYWHEEL;
                        break;

                    } // end if-else-if
                    
                } else 
                    break;
            /*
             * Clear Shooter State
             */
                
            case StateMachineState.CLE:
                
                if (isButtonPressed == true) {
                    
                    double start = Timer.getFPGATimestamp();

                    do {
                        shooter.manualTopIndexerDrive(-1);
                    } while (start + CLEAR_SHOOTER_TIME > Timer.getFPGATimestamp());

                    currentState = StateMachineState.SPIN_UP_FLYWHEEL;
                    break;
                    
                } else 
                    break;
                
            /*
             * Spin Up Shooter State
             */
                
            case StateMachineState.SPI:
                
                if (isButtonPressed == true) {
                    
                    double spinUpStart = Timer.getFPGATimestamp();

                    do {
                        shooter.spinUpShooter();
                    } while (spinUpStart + SPIN_UP_TIME > Timer.getFPGATimestamp());

                    currentState = StateMachineState.SHOOT;
                    break;
                    
                } else
                    break;
                
            /*
             * Shoot State
             */
                
            case StateMachineState.SHO:
                
                if (isButtonPressed = true) {
                    
                    double shootStart = Timer.getFPGATimestamp();

                    do {
                        shooter.indexerShoot();
                    } while (shootStart + 0.5 > Timer.getFPGATimestamp());

                    numShot++;

                    if (numShot == 3) {

                        currentState = StateMachineState.ILDE;
                        isFinished = true;
                        break;

                    } else {

                        currentState = StateMachineState.SPIN_UP_FLYWHEEL;
                        break;

                    } // end if-else
                    
                } else
                    break;
                
        } // end switch
        
    } // end execute

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return isFinished;
    }

    // Called once after isFinished returns true
    protected void end() {
        currentState = StateMachineState.ILDE;
    } // end end...lolz

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        end();
    } // end interrupted
}
